#include "pixelconv.h"







size_t pixel_bgr_to_bgra(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t* dst = (uint8_t*)output;
    uint8_t* src = (uint8_t*)input;
    uint8_t* dstend;
    size_t memsz = width * height * 4;
    dstend = dst + memsz;
    while(dst < dstend){   
        dst[0] = src[0]; //b
        dst[1] = src[1]; //b
        dst[2] = src[2]; //r
        dst[3] = 0xFF;  //a
        src += 3;
        dst += 4;
    }
    return memsz;    
}

size_t pixel_yuy2_to_bgra(const void *input, void *output, uint32_t width, uint32_t height) {
    uint8_t *dst = (uint8_t *)output;
    uint8_t *src = (uint8_t *)input;
    int z = 0, i = 0, j = 0;
    int r, g, b;
    int y, u, v;
    z = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (!z)
                y = src[0] << 8;
            else
                y = src[2] << 8;
            u = src[1] - 128;
            v = src[3] - 128;

            r = (y + (359 * v)) >> 8;
            g = (y - (88 * u) - (183 * v)) >> 8;
            b = (y + (454 * u)) >> 8;

            r = (r > 255) ? 255 : ((r < 0) ? 0 : r);
            g = (g > 255) ? 255 : ((g < 0) ? 0 : g);
            b = (b > 255) ? 255 : ((b < 0) ? 0 : b);

            *dst++ = b;
            *dst++ = g;
            *dst++ = r;
            *dst++ = 0xff;
            if (z++) {
                z = 0;
                src += 4;
            }
        }
    }
    return width * height * 4;
}



size_t pixel_yuy2_to_bgr(const void *input, void *output, uint32_t width, uint32_t height) {
    uint8_t *dst = (uint8_t *)output;
    uint8_t *src = (uint8_t *)input;
    int z = 0, i = 0, j = 0;
    int r, g, b;
    int y, u, v;
    z = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (!z)
                y = src[0] << 8;
            else
                y = src[2] << 8;
            u = src[1] - 128;
            v = src[3] - 128;

            r = (y + (359 * v)) >> 8;
            g = (y - (88 * u) - (183 * v)) >> 8;
            b = (y + (454 * u)) >> 8;

            r = (r > 255) ? 255 : ((r < 0) ? 0 : r);
            g = (g > 255) ? 255 : ((g < 0) ? 0 : g);
            b = (b > 255) ? 255 : ((b < 0) ? 0 : b);

            *dst++ = b;
            *dst++ = g;
            *dst++ = r;
            
            if (z++) {
                z = 0;
                src += 4;
            }
        }
    }
    return width * height * 3;
}

size_t pixel_yuy2_to_rgb(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t *dst = (uint8_t *)output;
    uint8_t *src = (uint8_t *)input;
    int z = 0, i = 0, j = 0;
    int r, g, b;
    int y, u, v;
    z = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (!z)
                y = src[0] << 8;
            else
                y = src[2] << 8;
            u = src[1] - 128;
            v = src[3] - 128;

            r = (y + (359 * v)) >> 8;
            g = (y - (88 * u) - (183 * v)) >> 8;
            b = (y + (454 * u)) >> 8;

            r = (r > 255) ? 255 : ((r < 0) ? 0 : r);
            g = (g > 255) ? 255 : ((g < 0) ? 0 : g);
            b = (b > 255) ? 255 : ((b < 0) ? 0 : b);

            *dst++ = r;
            *dst++ = g;
            *dst++ = b;
            
            if (z++) {
                z = 0;
                src += 4;
            }
        }
    }
    return width * height * 3;    
}



size_t pixel_yuy2_to_gray_rgb(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t *dst = (uint8_t *)output;
    uint8_t *src = (uint8_t *)input;
    int z = 0, i = 0, j = 0;
    int y;
    z = 0;
    for (i = 0; i < height; i++) {
        for (j = 0; j < width; j++) {
            if (!z)
                y = src[0];
            else
                y = src[2];
            
            *dst++ = y;
            *dst++ = y;
            *dst++ = y;
            
            if (z++) {
                z = 0;
                src += 4;
            }
        }
    }
    return width * height * 3;    
}


size_t pixel_bgr_to_rgb(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t* dst = (uint8_t*)output;
    uint8_t* src = (uint8_t*)input;
    uint8_t* dstend;
    size_t memsz = width * height * 3;
    dstend = dst + memsz;
    while(dst < dstend){   
        dst[0] = src[2]; //b
        dst[1] = src[1]; //b
        dst[2] = src[0]; //r        
        src += 3;
        dst += 3;
    }
    return memsz;     
}


size_t pixel_gray_reverse(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t* dst = (uint8_t*)output;
    uint8_t* src = (uint8_t*)input;
    uint8_t* dstend;
    size_t memsz = width * height;   
    dstend = dst + memsz;
    while(dst < dstend){   
        dst[0] = 255 - src[0];  
        src += 1;
        dst += 1;
    }
    return memsz;      
}


size_t pixel_rgb_reverse(const void* input, void* output, uint32_t width, uint32_t height){
    uint8_t* dst = (uint8_t*)output;
    uint8_t* src = (uint8_t*)input;
    uint8_t* dstend;
    size_t memsz = width * height * 3;   
    dstend = dst + memsz;  
    while(dst < dstend){   
        dst[0] = 255 - src[0];  
        src += 1;
        dst += 1;
    }
    return memsz;       
}


